/*
 * tmc222x.c
 *
 *  Created on: 2021-08-20
 *      Author: chenss
 */

#include "tmc222x.h"
#include "serial.h"
#include "etimer.h"
#include "shell.h"
#include "mstdio.h"
#include "utils.h"

#define SERIALx Serial_2

#define CURRENT_MIN_PERCENT (100 / 32)

#ifdef __DEBUG__
#   define DEFAULT_IHOLDIRUN_IRUN       (20 << TMC_IHOLDIRUN_IRUN_SHIFT)
#else
#   define DEFAULT_IHOLDIRUN_IRUN       (20 << TMC_IHOLDIRUN_IRUN_SHIFT)
#endif

#define DEFAULT_IHOLDIRUN_IHOLD      (5 << TMC_IHOLDIRUN_IHOLD_SHIFT)
#define DEFAULT_IHOLDIRUN_IHOLDDELAY (100 << TMC_IHOLDIRUN_IHOLDDELAY_SHIFT)

#define SGTHRS_ALIGN 18

// 根号2的数值
#define SQRT_2 1.4142135623730951

// Vfs电压(V)
#define V_FS 0.325

// Vref电压(V)
#define V_REF 0.167

// 采样电阻（欧姆）
#define R_SENSE 0.11

// 电机驱动电路的最大电流有效值
#define I_RMS_MAX (V_REF * V_FS / (SQRT_2 * (R_SENSE + 0.02) * 2.5))

static uint32_t _IHOLDIRUN;
static uint8_t _IFCNT;

#if 0

enum _RegAcces
{
    _RegAcces_None,
    _RegAcces_Read,
    _RegAcces_Write,
    _RegAcces_ReadWrite
};

static const int8_t _registerAccess[] =
{
//  0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F
    3, 3, 1, 2, 2, 1, 1, 3, 0, 0, 0, 0, 0, 0, 0, 0, // 0x00 - 0x0F
    2, 2, 1, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 0x10 - 0x1F
    0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 0x20 - 0x2F
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 0x30 - 0x3F
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 0x40 - 0x4F
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 0x50 - 0x5F
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 3, 0, 0, 1, // 0x60 - 0x6F
    3, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0  // 0x70 - 0x7F
};
#endif

static const struct
{
    uint8_t  reg;
    uint32_t value;
} _defaultRegValues[] =
{
    {TMC_GCONF,     TMC_GCONF_MULTISTEP_FILT | TMC_GCONF_PDN_DISABLE | TMC_GCONF_I_SCALE_ANALOG},
    {TMC_IHOLDIRUN, DEFAULT_IHOLDIRUN_IHOLDDELAY | DEFAULT_IHOLDIRUN_IRUN | DEFAULT_IHOLDIRUN_IHOLD},
    {TMC_TPWMTHRS, 0},
    {TMC_TCOOLTHRS, 0x300},
    {TMC_SGTHRS, SGTHRS_ALIGN},
};

static bool _OnRx(Serial_t serial, uint8_t b);
static void _PrintRegister(void *stream, uint8_t reg, uint32_t value);

int TMC222x_Init(void)
{
    uint8_t reg;
    uint32_t value;
    int ret = -1;

    nbdebug("%s\n", __func__);

#ifdef CMSIS_OS
    const osMutexAttr_t attr = {NULL, osMutexPrioInherit | osMutexRecursive, NULL, 0};

    _mutex = osMutexNew(&attr);
#endif

    Serial_SetCallbacks(SERIALx, NULL, NULL, _OnRx);

    nbprintf("Read IFCNT ");
    ret = TMC222x_ReadReg(TMC_IFCNT, &value);
    if(ret == 0)
    {
        _IFCNT = (uint8_t)value;
        nbprintf("= %u\n", (unsigned int)_IFCNT);
    }
    else
    {
        nbprintf("fail: %d\n", ret);
        return ret;
    }

    for(unsigned int i = 0; i < ARRAY_LEN(_defaultRegValues); i++)
    {
        reg = _defaultRegValues[i].reg;
        value = _defaultRegValues[i].value;
        ret = TMC222x_WriteReg(reg, value);
        if(ret != 0)
        {
            nbprintf("TMC222x: write register: 0x%02x fail: %d\n", (unsigned)reg, ret);
        }
    }

    return ret;
}

static uint8_t _CalcCRC(const void *data, unsigned int size)
{
    unsigned int i, j;
    uint8_t b;
    const uint8_t *vdata = data;
    uint8_t crc = 0;

    for(i = 0; i < size; i++)
    {
        b = vdata[i];
        for(j = 0; j < 8; j++)
        {
            if((crc >> 7) ^ (b & 0x01))
            {
                crc = (crc << 1) ^ 0x07;
            }
            else
            {
                crc = (crc << 1);
            }

            b >>= 1;
        } // for CRC bit
    } // for message byte

    return crc;
}

static inline int _Send(const void *data, unsigned int size)
{
    uint8_t crc;
    uint8_t rxBuf[size + 1];
    unsigned int n;

    crc = _CalcCRC(data, size);

    Serial_IoCtl(SERIALx, Serial_Ctl_ClearRxBuffer);
    Serial_Send(SERIALx, data, size);
    Serial_SendByte(SERIALx, crc);

    // Tx与Rx引脚在链路上相连，发送完成后应该接收到同样的内容
    n = Serial_Recv(SERIALx, rxBuf, sizeof(rxBuf), 100);
    if(n != sizeof(rxBuf))
    {
        return -1;
    }

    if((memcmp(rxBuf, data, size) != 0) || (rxBuf[size] != crc))
    {
        return -2;
    }

    return 0;
}

/**
 * @brief 读取寄存器值
 * @param reg   寄存器地址
 * @param value 寄存器数值缓存
 * @return 0表示成功，否则表示失败
 */
int TMC222x_ReadReg(uint8_t reg, uint32_t *value)
{
    int ret = 0;
    unsigned int n;
    uint8_t rxCrc, crc;
    uint8_t rxBuf[8];
    uint8_t txBuf[] =
    {
        0x05,
        0x00,
        reg
    };

#ifdef CMSIS_OS
    ret = osMutexAcquire(_mutex, osWaitForever);
    if(ret != osOK)
    {
        return ret;
    }
#endif

    do
    {
        *value = 0;
        ret = _Send(txBuf, sizeof(txBuf));
        if(ret != 0)
        {
            break;
        }

        n = Serial_Recv(SERIALx, rxBuf, sizeof(rxBuf), 100);
        if(n != sizeof(rxBuf))
        {
            ret = -2;
            break;
        }

        rxCrc = rxBuf[sizeof(rxBuf) - 1];
        crc = _CalcCRC(rxBuf, sizeof(rxBuf) - 1);
        if((rxBuf[1] != 0xff) || (rxBuf[2] != reg) || (crc != rxCrc))
        {
            ret = -3;
#if __DEBUG__
            nbprintf("[");
            for(unsigned int i = 0; i < sizeof(rxBuf); i ++)
            {
                nbprintf("%02x ", rxBuf[i]);
            }
            nbprintf("]");
#endif
            break;
        }

        *value = (((uint32_t)(rxBuf[3])) << 24) | (((uint32_t)(rxBuf[4])) << 16) | (((uint32_t)(rxBuf[5])) << 8) | rxBuf[6];
    }while(0);

#ifdef CMSIS_OS
    osMutexRelease(_mutex);
#endif
    return ret;
}

/**
 *
 * @param reg
 * @param value
 * @return
 */
int TMC222x_WriteReg(uint8_t reg, uint32_t value)
{
    int ret;
    uint32_t v;
    uint8_t ifcnt = _IFCNT + 1;
    uint8_t buf[] =
    {
        0xa5,
        0x00,
        reg | TMC_WRITE_BIT,
        (uint8_t)(value >> 24),
        (uint8_t)(value >> 16),
        (uint8_t)(value >> 8),
        (uint8_t)value
    };

#ifdef CMSIS_OS
    ret = osMutexAcquire(_mutex, osWaitForever);
    if(ret != osOK)
    {
        return ret;
    }

    _task =  osThreadGetId();
    ret = _Send(buf, sizeof(buf));
    if(ret == 0)
    {
        if(reg == TMC_IHOLDIRUN)
        {
            _IHOLDIRUN = value;
        }

        if(osThreadFlagsWait(0x01, osFlagsWaitAny, OS_TICK_MS(100)) != 0x01)
        {
            ret = osErrorTimeout;
        }
    }

    _task = NULL;
    osMutexRelease(_mutex);
#else
    ret = _Send(buf, sizeof(buf));
    if(ret != 0)
    {
        return ret;
    }

    ret = TMC222x_ReadReg(TMC_IFCNT, &v);
    if(ret != 0)
    {
        return ret;
    }

    _IFCNT = (uint8_t)v;
    if(_IFCNT != ifcnt)
    {
        return -1;
    }

    if(reg == TMC_IHOLDIRUN)
    {
        _IHOLDIRUN = value;
    }
#endif

    return ret;
}

static bool _OnRx(Serial_t serial, uint8_t b)
{
    UNUSED(serial);
    UNUSED(b);
//    ON_DEBUG(mprintf("%02x ", b);)

    return true;
}

/**
 * @brief 设置电机在运行和停止(standstill)时电流大小
 * @param run 运行电流（%）
 * @param hold 停止电流（%）
 * @return 0 设置成功，否则失败
 */
int TMC222x_SetCurrent(int run, int hold)
{
    uint32_t iRun, iHold;
    uint32_t min = CURRENT_MIN_PERCENT;
    uint32_t v = _IHOLDIRUN;

    if((run < 0) && (hold < 0))
    {
        return -1;
    }

    if(run >= 0)
    {
        if(run > 100)
        {
            run = 100;
        }
        else if(run < min)
        {
            run = min;
        }

        iRun  = Utils_InterpolateInt(min, 0, 100, TMC_IHOLDIRUN_IRUN_MAX, run);
        BITS_SET(v, TMC_IHOLDIRUN_IRUN_MASK, iRun << TMC_IHOLDIRUN_IRUN_SHIFT);
    }

    if(hold >= 0)
    {
        if(hold > 100)
        {
            hold = 100;
        }
        else if(hold < min)
        {
            hold = min;
        }

        iHold = Utils_InterpolateInt(min, 0, 100, TMC_IHOLDIRUN_IHOLD_MAX, hold);
        BITS_SET(v, TMC_IHOLDIRUN_IHOLD_MASK, iHold << TMC_IHOLDIRUN_IHOLD_SHIFT);
    }

    return TMC222x_WriteReg(TMC_IHOLDIRUN, v);
}

static void _PrintRegister(void *stream, uint8_t reg, uint32_t value)
{
    switch(reg)
    {
    case TMC_IOIN:
        chprintf(stream, "IOIN = 0x%08lx\n", value);
        chprintf(stream, "SPREAD: %c\n", (value & TMC_IOIN_SPREAD) ? '1' : '0');
        chprintf(stream, "DIR: %c\n", (value & TMC_IOIN_DIR) ? '1' : '0');
        chprintf(stream, "ENN: %c\n", (value & TMC_IOIN_ENN) ? '1' : '0');
        chprintf(stream, "STEP: %c\n", (value & TMC_IOIN_STEP) ? '1' : '0');
        chprintf(stream, "MS1: %c\n", (value & TMC_IOIN_MS1) ? '1' : '0');
        chprintf(stream, "MS2: %c\n", (value & TMC_IOIN_MS2) ? '1' : '0');
        chprintf(stream, "IC: %s\n", (value & TMC_IOIN_SEL_A) ? "TMC220x" : "TMC222x");
        break;

    case TMC_DRVSTATUS:
        chprintf(stream, "DRVSTATUS = 0x%08lx\n", value);
        chprintf(stream, "standstill: %c\n", (value & TMC_DRVSTATUS_STST) ? '1' : '0');
        chprintf(stream, "stealth: %c\n", (value & TMC_DRVSTATUS_STEALTH) ? '1' : '0');
        chprintf(stream, "t157: %c\n", (value & TMC_DRVSTATUS_T157) ? '1' : '0');
        chprintf(stream, "t150: %c\n", (value & TMC_DRVSTATUS_T150) ? '1' : '0');
        chprintf(stream, "t143: %c\n", (value & TMC_DRVSTATUS_T143) ? '1' : '0');
        chprintf(stream, "t120: %c\n", (value & TMC_DRVSTATUS_T120) ? '1' : '0');
        chprintf(stream, "open load B: %c\n", (value & TMC_DRVSTATUS_OLB) ? '1' : '0');
        chprintf(stream, "open load A: %c\n", (value & TMC_DRVSTATUS_OLA) ? '1' : '0');
        chprintf(stream, "low side short B: %c\n", (value & TMC_DRVSTATUS_S2VSB) ? '1' : '0');
        chprintf(stream, "low side short A: %c\n", (value & TMC_DRVSTATUS_S2VSA) ? '1' : '0');
        chprintf(stream, "short to ground B: %c\n", (value & TMC_DRVSTATUS_S2GB) ? '1' : '0');
        chprintf(stream, "short to ground A: %c\n", (value & TMC_DRVSTATUS_S2GA) ? '1' : '0');
        chprintf(stream, "over temperature: %c\n", (value & TMC_DRVSTATUS_OT) ? '1' : '0');
        chprintf(stream, "over temperature pre-warning: %c\n", (value & TMC_DRVSTATUS_OTPW) ? '1' : '0');
        {
            unsigned int v = (unsigned)TMC_DRVSTATUS_CS_ACTUAL(value);
            unsigned int percent = 100 * (v + 1) / 32;

            chprintf(stream, "actual current: %u as %u%%\n", v, percent);
        }
        break;

    default:
        chprintf(stream, "0x%08lx\n", value);
    }
}

static int _ShellCmdTMC(void *stream, int argc, char *argv[])
{
    uint8_t reg = 0;
    uint32_t v = 0;
    bool isWrite = false;
    int ret;

    switch(argc)
    {
    default:
    case 3:
        v = (uint32_t)Utils_StrToNumber(argv[2]);
        isWrite = true;
        /* no break */
    case 2:
        reg = (uint8_t)Utils_StrToNumber(argv[1]);
        break;

    case 1:
        Shell_Help(stream, argv[0]);
        return 0;

    case 0:
        return -1;
    }

    if(isWrite)
    {
        chprintf(stream, "write 0x%08lx to register %u: ", v, (unsigned int)reg);
        ret = TMC222x_WriteReg(reg, v);
        if(ret == 0)
        {
            chprintf(stream, "success\n");
        }
        else
        {
            chprintf(stream, "fail: %d\n", ret);
        }
    }
    else
    {
        chprintf(stream, "read register 0x%02x: ", (unsigned int)reg);
        ret = TMC222x_ReadReg(reg, &v);
        if(ret == 0)
        {
            _PrintRegister(stream, reg, v);
        }
        else
        {
            chprintf(stream, "fail: %d\n", ret);
        }
    }

    return 0;
}

SHELL_CMD(tmc, _ShellCmdTMC, "read or write TMC222x register", "<register address> [<register value>]");

static int _ShellCmdCurrent(void *stream, int argc, char *argv[])
{
    int run = -1, hold = -1;
    int ret = 0;

    switch(argc)
    {
    default:
    case 3:
        hold = (int)Utils_StrToNumber(argv[2]);
        /* no break */
    case 2:
        run = (int)Utils_StrToNumber(argv[1]);
        chprintf(stream, "Set motor current: ");
        if(run >= 0)
        {
            chprintf(stream, "run = %u%%, ", run);
        }

        if(hold >= 0)
        {
            chprintf(stream, "hold = %u%%, ", hold);
        }
        ret = TMC222x_SetCurrent(run, hold);
        if(ret == 0)
        {
            chprintf(stream, "success\n");
        }
        else
        {
            chprintf(stream, "fail: %d\n", ret);
        }
        break;

    case 1:
    {
        uint32_t v = _IHOLDIRUN;
        float iRun, iHold;

        run  = 100 * (TMC_IHOLDIRUN_IRUN(v)  + 1) / 32;
        hold = 100 * (TMC_IHOLDIRUN_IHOLD(v) + 1) / 32;
        iRun  = I_RMS_MAX * (TMC_IHOLDIRUN_IRUN(v)  + 1) / 32.0;
        iHold = I_RMS_MAX * (TMC_IHOLDIRUN_IHOLD(v) + 1) / 32.0;
        chprintf(stream, "Motor current setting: run = %d%%(%.2fA), hold = %d%%(%.2fA)\n", run, iRun, hold, iHold);
    }
        break;

    case 0:
        ret = -1;
    }

    return ret;
}

SHELL_CMD(current, _ShellCmdCurrent, "Show or set motor current.", "[<run current(%)> [<hold current(%)>]]");
